/**
 *@note Copyright 2012-2022 CETHIK All Rights Reserved.
 *@brief  地图管理类文件
 *@author tuyinan
 *@data 2022-04-xx
 */
#ifndef COSTMAP_2D_COSTMAP_2D_H_
#define COSTMAP_2D_COSTMAP_2D_H_

#include <costmap_2d/msg_struct.h>

#include <boost/thread.hpp>
#include <iostream>
#include <queue>
#include <vector>

namespace costmap_2d {

/// 地图点表示结构
struct MapLocation {
  unsigned int x;
  unsigned int y;
};
/**
 *@brief 代价地图管理类
 *@author tuyinan
 *@version 2022-04-xx
 */
class Costmap2D {
 public:
  /**
   *@brief 构造函数
   *@param unsigned int cells_size_x 地图横向块数
   *@param unsigned int cells_size_y 地图纵向快数
   *@param  double resolution 块的实际比例
   *@param  double origin_x 地图origin x
   *@param  double origin_y 地图origin y
   *@param  unsigned char default_value = 0 代价默认值
   *@return void
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  Costmap2D(unsigned int cells_size_x, unsigned int cells_size_y,
            double resolution, double origin_x, double origin_y,
            unsigned char default_value = 0);

  /**
   *@brief 构造函数
   *@param const Costmap2D& map 代价地图
   *@return void
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  Costmap2D(const Costmap2D& map);
  /**
   *@brief 重构等号
   *@param const Costmap2D& map 代价地图
   *@return void
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  Costmap2D& operator=(const Costmap2D& map);
  /**
   *@brief 把地图放到一个window区域内
   *@param const Costmap2D& map 代价地图
   *@param double win_origin_x window 开始位置x
   *@param double win_origin_y window 开始位置y
   *@param double win_size_x window 的大小x
   *@param double win_size_y  double win_size_y
   *@return bool 成功或失败
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  bool copyCostmapWindow(const Costmap2D& map, double win_origin_x,
                         double win_origin_y, double win_size_x,
                         double win_size_y);
  /**
   *@brief 默认构造函数
   *@param 无
   *@return void
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  Costmap2D();
  /**
   *@brief 析构函数
   *@param 无
   *@return void
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  virtual ~Costmap2D();
  /**
   *@brief 从代价图中获取值
   *@param unsigned int mx x坐标
   *@param unsigned int my y坐标
   *@return  unsigned char 值
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  unsigned char getCost(unsigned int mx, unsigned int my) const;
  /**
   *@brief 设置代价图中的代价值
   *@param unsigned int mx x坐标
   *@param unsigned int my y坐标
   *@param unsigned char cost 代价值
   *@return void
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  void setCost(unsigned int mx, unsigned int my, unsigned char cost);
  /**
   *@brief 转换地图坐标到世界坐标
   *@param unsigned int mx 地图坐标x
   *@param unsigned int my 地图坐标y
   *@param double& wx 世界坐标x
   *@param double& wy 世界坐标y
   *@return void
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  void mapToWorld(unsigned int mx, unsigned int my, double& wx,
                  double& wy) const;

  /**
   *@brief 转换地图坐标到世界坐标
   *@param unsigned int& mx 地图坐标x
   *@param unsigned int& my 地图坐标y
   *@param double wx 世界坐标x
   *@param double wy 世界坐标y
   *@return bool 成功或失败
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  bool worldToMap(double wx, double wy, unsigned int& mx,
                  unsigned int& my) const;

  /**
   *@brief 转换地图坐标到世界坐标（无视边界约束）
   *@param unsigned int& mx 地图坐标x
   *@param unsigned int& my 地图坐标y
   *@param double wx 世界坐标x
   *@param double wy 世界坐标y
   *@return void
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  void worldToMapNoBounds(double wx, double wy, int& mx, int& my) const;

  /**
   *@brief 转换地图坐标到世界坐标（考虑边界约束）
   *@param unsigned int& mx 地图坐标x
   *@param unsigned int& my 地图坐标y
   *@param double wx 世界坐标x
   *@param double wy 世界坐标y
   *@return void
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  void worldToMapEnforceBounds(double wx, double wy, int& mx, int& my) const;

  /**
   *@brief 根据坐标获取编号，地图中是一维排列的
   *@param unsigned int mx 地图坐标x
   *@param unsigned int my 地图坐标y
   *@return unsigned int 编号
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  inline unsigned int getIndex(unsigned int mx, unsigned int my) const {
    return my * size_x_ + mx;
  }

  /**
   *@brief 根据编号获取坐标
   *@param unsigned int& mx 地图坐标x
   *@param unsigned int& my 地图坐标y
   *@param unsigned int 编号
   *@return void
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  inline void indexToCells(unsigned int index, unsigned int& mx,
                           unsigned int& my) const {
    my = index / size_x_;
    mx = index - (my * size_x_);
  }
  /**
   *@brief 获取char map 指针
   *@param 无
   *@return unsigned char* 地图指针
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  unsigned char* getCharMap() const;
  /**
   *@brief 获取地图横坐标上有几个cell
   *@param 无
   *@return unsigned int 个数
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  unsigned int getSizeInCellsX() const;

  /**
   *@brief 获取地图纵坐标上有几个cell
   *@param 无
   *@return unsigned int 个数
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  unsigned int getSizeInCellsY() const;

  /**
   *@brief 获取地图长度
   *@param 无
   *@return double 米数
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  double getSizeInMetersX() const;

  /**
   *@brief 获取地图宽度
   *@param 无
   *@return double 米数
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  double getSizeInMetersY() const;

  /**
   *@brief 获取地图起点x
   *@param 无
   *@return double x值
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  double getOriginX() const;

  /**
   *@brief 获取地图起点y
   *@param 无
   *@return double y值
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  double getOriginY() const;

  /**
   *@brief 获取resolution
   *@param 无
   *@return double 实际与地图转换值
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  double getResolution() const;
  /**
   *@brief 设置初始代价值
   *@param unsigned char c 值
   *@return void
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  void setDefaultValue(unsigned char c) { default_value_ = c; }
  /**
   *@brief 获取初始代价值
   *@param 无
   *@return unsigned char c 值
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  unsigned char getDefaultValue() { return default_value_; }
  /**
   *@brief 把机器人占位所代表的的地图格代价给予值
   *@param const std::vector<Point>& polygon 占位
   *@param unsigned char cost_value 代价值
   *@return bool 成功或失败
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  bool setConvexPolygonCost(const std::vector<Point>& polygon,
                            unsigned char cost_value);
  /**
   *@brief 计算在占位边界的格子
   *@param const std::vector<MapLocation>& polygon 占位
   *@param std::vector<MapLocation>& polygon_cells 格子集合
   *@return void
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  void polygonOutlineCells(const std::vector<MapLocation>& polygon,
                           std::vector<MapLocation>& polygon_cells);

  /**
   *@brief 计算在占位里的格子
   *@param const std::vector<MapLocation>& polygon 占位
   *@param std::vector<MapLocation>& polygon_cells 格子集合
   *@return void
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  void convexFillCells(const std::vector<MapLocation>& polygon,
                       std::vector<MapLocation>& polygon_cells);
  /**
   *@brief 更新origin
   *@param double new_origin_x 新的origin的x
   *@param double new_origin_y 新的origin的y
   *@return void
   *@author: tuyinan
   *@version: 2022--xx
   */
  virtual void updateOrigin(double new_origin_x, double new_origin_y);
  /**
   *@brief 存储地图
   *@param std::string file_name 存储地址文件名
   *@return bool 成功或失败
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  bool saveMap(std::string file_name);
  /**
   *@brief resize地图
   *@param unsigned int size_x  长度
   *@param unsigned int size_y  宽度
   *@param double resolution 实际与地图转换值
   *@param double origin_x 地图起始点的x
   *@param double origin_y 地图起始点的y
   *@return void
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  void resizeMap(unsigned int size_x, unsigned int size_y, double resolution,
                 double origin_x, double origin_y);
  /**
   *@brief 重置地图
   *@param unsigned int x0 左上角x
   *@param unsigned int y0 左上角y
   *@param unsigned int xn 右下角x
   *@param unsigned int yn 右下角y
   *@return void
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  void resetMap(unsigned int x0, unsigned int y0, unsigned int xn,
                unsigned int yn);
  /**
   *@brief 实际距离转换为地图格子距离
   *@param double world_dist 实际距离
   *@return unsigned int 格子数
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  unsigned int cellDistance(double world_dist);

  typedef boost::recursive_mutex mutex_t;
  /**
   *@brief 获取锁
   *@param 无
   *@return mutex_t* 锁的指针
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  mutex_t* getMutex() { return access_; }

 protected:
  /**
   *@brief 复制地图，从原图中的一部分复制到目的地图中
   *@param data_type* source_map 原图
   *@param unsigned int sm_lower_left_x 原图的开始复制点的x
   *@param unsigned int sm_lower_left_y 原图的开始复制点的y
   *@param unsigned int sm_size_x 原图的宽
   *@param data_type* dest_map 目的地图
   *@param unsigned int dm_lower_left_x 目的地图的开始复制点的x
   *@param unsigned int dm_lower_left_y 目的地图的开始复制点的y
   *@param unsigned int dm_size_x 目的地图的宽
   *@param unsigned int region_size_x 要复制的部分的宽度
   *@param unsigned int region_size_y 要复制的部分的高度
   *@return void
   *@author: tuyinan
   *@version: 2022--xx
   */
  template <typename data_type>
  void copyMapRegion(data_type* source_map, unsigned int sm_lower_left_x,
                     unsigned int sm_lower_left_y, unsigned int sm_size_x,
                     data_type* dest_map, unsigned int dm_lower_left_x,
                     unsigned int dm_lower_left_y, unsigned int dm_size_x,
                     unsigned int region_size_x, unsigned int region_size_y) {
    // we'll first need to compute the starting points for each map
    data_type* sm_index =
        source_map + (sm_lower_left_y * sm_size_x + sm_lower_left_x);
    data_type* dm_index =
        dest_map + (dm_lower_left_y * dm_size_x + dm_lower_left_x);

    // now, we'll copy the source map into the destination map
    for (unsigned int i = 0; i < region_size_y; ++i) {
      memcpy(dm_index, sm_index, region_size_x * sizeof(data_type));
      sm_index += sm_size_x;
      dm_index += dm_size_x;
    }
  }

  /**
   *@brief 删除地图
   *@param 无
   *@return void
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  virtual void deleteMaps();
  /**
   *@brief 重置地图
   *@param 无
   *@return void
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  virtual void resetMaps();
  /**
   *@brief 初始化地图
   *@param unsigned int size_x 地图宽度
   *@param unsigned int size_y 地图高度
   *@return void
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  virtual void initMaps(unsigned int size_x, unsigned int size_y);
  /**
   *@brief 采用模板方法对一条线内的点进行操作
   *@param ActionType at 动作类，采用的模板
   *@param unsigned int x0 开始的点的x
   *@param unsigned int y0 开始的点的y
   *@param unsigned int x1 结束的点的x
   *@param unsigned int y1 结束的点的y
   *@param unsigned int max_length 分割的长度
   *@return void
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  template <class ActionType>
  inline void raytraceLine(ActionType at, unsigned int x0, unsigned int y0,
                           unsigned int x1, unsigned int y1,
                           unsigned int max_length = UINT_MAX) {
    int dx = x1 - x0;
    int dy = y1 - y0;

    unsigned int abs_dx = abs(dx);
    unsigned int abs_dy = abs(dy);

    int offset_dx = sign(dx);
    int offset_dy = sign(dy) * size_x_;

    unsigned int offset = y0 * size_x_ + x0;

    // we need to chose how much to scale our dominant dimension, based on the
    // maximum length of the line
    double dist = hypot(dx, dy);
    double scale = (dist == 0.0) ? 1.0 : std::min(1.0, max_length / dist);

    // if x is dominant
    if (abs_dx >= abs_dy) {
      int error_y = abs_dx / 2;
      bresenham2D(at, abs_dx, abs_dy, error_y, offset_dx, offset_dy, offset,
                  (unsigned int)(scale * abs_dx));
      return;
    }

    // otherwise y is dominant
    int error_x = abs_dy / 2;
    bresenham2D(at, abs_dy, abs_dx, error_x, offset_dy, offset_dx, offset,
                (unsigned int)(scale * abs_dy));
  }

 private:
  /**
   *@brief Bresenham's raytracing algorithm的2d实现，对每个分段进行操作
   *@param ActionType at 动作类，采用的模板
   *@param unsigned int abs_da 一个维度上的绝对距离
   *@param unsigned int abs_db 另一个维度上的绝对距离
   *@param int error_b 基准差
   *@param int offset_a 第一个维度的前缀
   *@param int offset_b 另一个维度的前缀
   *@param unsigned int offset 起始点
   *@param unsigned int max_length 分割的长度
   *@return void
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  template <class ActionType>
  inline void bresenham2D(ActionType at, unsigned int abs_da,
                          unsigned int abs_db, int error_b, int offset_a,
                          int offset_b, unsigned int offset,
                          unsigned int max_length) {
    unsigned int end = std::min(max_length, abs_da);
    for (unsigned int i = 0; i < end; ++i) {
      at(offset);
      offset += offset_a;
      error_b += abs_db;
      if ((unsigned int)error_b >= abs_da) {
        offset += offset_b;
        error_b -= abs_da;
      }
    }
    at(offset);
  }
  /**
   *@brief 经典sign公式
   *@param int x 值
   *@return int 1或-1
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  inline int sign(int x) { return x > 0 ? 1.0 : -1.0; }
  ///一把小锁
  mutex_t* access_;

 protected:
  /// 宽
  unsigned int size_x_;
  /// 高
  unsigned int size_y_;
  ///映射
  double resolution_;
  ///起始点的x
  double origin_x_;
  ///起始点的y
  double origin_y_;
  ///代价地图一维
  unsigned char* costmap_;
  /// 代价地图初始默认值
  unsigned char default_value_;
  /**
   *@brief 普通动作类
   *@author tuyinan
   *@version 2022-04-xx
   */
  class MarkCell {
   public:
    /**
     *@brief 构造函数
     *@param unsigned char* costmap 代价地图
     *@param unsigned char value 值
     *@return void
     *@author: tuyinan
     *@version: 2022--xx
     */
    MarkCell(unsigned char* costmap, unsigned char value)
        : costmap_(costmap), value_(value) {}
    /**
     *@brief 重构小括号，直接替代地图上的值
     *@param unsigned int offset 位置
     *@return void
     *@author: tuyinan
     *@version: 2022-04-xx
     */
    inline void operator()(unsigned int offset) { costmap_[offset] = value_; }

   private:
    ///代价地图
    unsigned char* costmap_;
    ///值
    unsigned char value_;
  };
  /**
   *@brief 占位边际动作类
   *@author tuyinan
   *@version 2022-04-xx
   */
  class PolygonOutlineCells {
   public:
    /**
     *@brief 构造函数
     *@param const Costmap2D& costmap 代价地图类
     *@param const unsigned char* char_map 一维代价地图
     *@param std::vector<MapLocation>& cells 点集合
     *@return void
     *@author: tuyinan
     *@version: 2022-04-xx
     */
    PolygonOutlineCells(const Costmap2D& costmap, const unsigned char* char_map,
                        std::vector<MapLocation>& cells)
        : costmap_(costmap), char_map_(char_map), cells_(cells) {}
    /**
     *@brief 重构小括号，将点转换为物理坐标记录
     *@param unsigned int offset 位置
     *@return void
     *@author: tuyinan
     *@version: 2022-04-xx
     */
    inline void operator()(unsigned int offset) {
      MapLocation loc;
      costmap_.indexToCells(offset, loc.x, loc.y);
      cells_.push_back(loc);
    }

   private:
    ///代价地图类
    const Costmap2D& costmap_;
    ///一维代价地图
    const unsigned char* char_map_;
    /// 实际点存储集合
    std::vector<MapLocation>& cells_;
  };
};
}  // namespace costmap_2d

#endif  // COSTMAP_2D_COSTMAP_2D_H
